#include "shipalgorithm.h"
#include "Mercator.h"
#include "cmath"
#include "geographicutil.h"

using namespace ns_yht;

ShipAlgorithm::ShipAlgorithm()
{
}

void ShipAlgorithm::GetNewPosition(double lat1, double lon1, double distance, double angle, double &lat2, double &lon2)
{
    // using namespace enav;

    // double x1 = Mercator::ToX(lon1);
    // double y1 = Mercator::ToY(lat1);

    // double x2 = x1 + distance * sin(angle);
    // double y2 = y1 + distance * cos(angle);

    // lon2 = Mercator::ToLon(x2);
    // lat2 = Mercator::ToLat(y2);
    GeographicUtil::getCoordinate(lat1, lon1, angle, distance, lat2, lon2);
}

void ShipAlgorithm::getAngleDis(double lat1, double lon1, double angle1, double lat2, double lon2, double &angle, double &dis)
{
    // double ownx = Mercator::ToX(lon1);
    // double owny = Mercator::ToY(lat1);

    // double otherx = Mercator::ToX(lon2);
    // double othery = Mercator::ToY(lat2);

    // double deltax = otherx - ownx;
    // double deltay = othery - owny;
    // dis = sqrt(deltax * deltax + deltay * deltay);

    // angle = atan2(deltax, deltay) * 180. / Mercator::PI;
    // if (angle < 0)
    //     angle = angle + 360; // 调整区间到0-360
    // angle = angle - angle1;

    DoublePair coor0(lat1, lon1);
    DoublePair coor1(lat2, lon2);

    dis = GeographicUtil::getGeoDistance(coor0, coor1, true);
    angle = GeographicUtil::getGeoAngle(coor0, coor1);
    if (angle < 0)
        angle += 360; // 调整区间到0-360
    angle = angle - angle1;
}
